#include "svpwm.hpp"
#include "math.h"
#include <stdio.h>

SVPWM::Result SVPWM::calculate(double alpha,double beta){
    Result result;
    char x_code[6] = {
        4,6,2,3,1,5
    };
    char y_code[6] = {
        6,2,3,1,5,4
    };
    /* 计算电角度. */
    double theta = atan2(beta,alpha);
    if ( theta < 0 ){
        theta = theta + M_PI * 2;
    }
    int sector = (int)(theta * 3.0 / M_PI);
    double m = sqrt(alpha * alpha + beta * beta) * 1.73205080757 / 1.0;
    /* 限制m的范围. */
    if ( m > 1 ){
        m = 1;
    }
    /* 归化角度. */
    theta = theta - (M_PI * sector / 3);
    /* 归化扇区. */
    if ( sector < 0 ){
        sector += 6;
    }else if ( sector >= 6 ){
        sector -= 6;
    }
    /* 计算px与py. */
    double p_x = m * sin(M_PI / 3 - theta);
    double p_y = m * sin(theta);
    double p_0 = 1 - p_x - p_y;

    result.p_a = p_x * (x_code[sector] & 0x01) + p_y * (y_code[sector] & 0x01) + p_0 / 2;
    result.p_b = p_x * ((x_code[sector] >> 1) & 0x01) + p_y * ((y_code[sector] >> 1) & 0x01) + p_0 / 2;
    result.p_c = p_x * ((x_code[sector] >> 2) & 0x01) + p_y * ((y_code[sector] >> 2) & 0x01) + p_0 / 2;
    
    return result;
}

